import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;


/**Procedurale WallFollower **/

public class GetDistancesTest {
	private static final boolean VERBOSE = false;
	private static int sleepTime = 100;

	public static void main(String [] args) throws InterruptedException{		
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 164;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		TachoPilot pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		UltrasonicSensor sonicSensor = new UltrasonicSensor(SensorPort.S2);	
		
		int [] distances = new int[8];
		sonicSensor.ping();
		Thread.sleep(500);
		sonicSensor.getDistances(distances);
		Thread.sleep(500);
		for(int i = 0; i < 8; i++){
			LCD.drawString("dist: " + distances[i], 0, i);
			Thread.sleep(500);
		}
		LCD.clearDisplay();
		for(int j = 0; j < 8; j++){
			distances[j] = 9000;
		}
		LCD.clearDisplay();
		for(int k = 0; k < 8; k++){
			LCD.drawString("dist: " + distances[k], 0, k);
			Thread.sleep(500);
			k++;
		}
		LCD.clearDisplay();
		LCD.drawString("getDist", 0, 0);
		int distance = sonicSensor.getDistance();
		LCD.drawString(""+distance, 0, 0);
		Thread.sleep(1000);				
	}
}